package legway.tests;

import lejos.nxt.ADSensorPort;
import lejos.nxt.LCD;
import lejos.nxt.SensorConstants;

/**
 * A class for gyro sensors.
 * 
 * @author Andreas Koefoed-Hansen, Tim Rasmussen, Kim Munk Petersen
 */
public class GyroSensor implements SensorConstants {

	protected ADSensorPort port;
	private final int CALIBRATION_SAMPLES = 200;
	private volatile static double offset;
	private volatile static int intOffset;
	private volatile static double angle;
	private volatile static double angularSpeed;

	public GyroSensor(ADSensorPort port, double offset) {
		port.setTypeAndMode(TYPE_CUSTOM, MODE_RAW);
		this.port = port;
		this.offset = offset;
		new Integrator().start();
	}

	public GyroSensor(ADSensorPort port) {
		this.port = port;
		port.setTypeAndMode(TYPE_CUSTOM, MODE_RAW);

		// Calibrate: find correct offset experimentally
		calibrate();
		new Integrator().start();
	}

	/**
	 * Read the gyro value i.e. angular speed.
	 * If a HiTechnic gyroscope is used, the unit is degrees/second
	 * 
	 * @return The gyroscope's angular speed
	 */
	public double getAngularSpeed() { 
		return angularSpeed; 
	}

	/**
	 * An approximation of the angle of the gyroscope
	 * 
	 * @return The angle of the gyroscope
	 */
	public double getAngle() { 
		return angle; 
	}

	/**
	 * Find the actual offset by using an average of samples
	 * The gyro must be held completely still during calibration!
	 */
	public void calibrate() {
		int sum = 0;
		for(int i = 0; i < CALIBRATION_SAMPLES; i++) {
			sum += port.readRawValue();
			try {
				Thread.sleep(5);
			} catch (InterruptedException e) { }
		}
		intOffset = (int)Math.round((double)sum/(double)CALIBRATION_SAMPLES);
		offset = ((double)(sum-intOffset*CALIBRATION_SAMPLES)/(double)CALIBRATION_SAMPLES);
	}

	/**
	 * Set the offset manually
	 */
	public void setOffset(double offset) {
		this.offset = offset;
	}

	private class Integrator extends Thread
	{
		long timeLast;
		long timeNow;

		public Integrator() {
			this.setDaemon(true);
			angularSpeed = port.readRawValue() - intOffset - offset;
			this.timeLast = System.currentTimeMillis();
		}

		public void run() {
			LCD.drawString("offs: " + offset + "           ", 0, 5);
			while(true) {
				timeNow = System.currentTimeMillis();
				angularSpeed = port.readRawValue() - intOffset - offset;
				angle += ((timeNow - timeLast) * angularSpeed)/1000.0;
				timeLast = timeNow;
				try {
					Thread.sleep(4);
				} catch (InterruptedException e) { }
			}
		}
	}
}
